from enum import Enum
import simplepyble
import numpy
import threading
import uuid
import time


class _INFO(Enum):
    """Enum containing info UUIDs"""
    SERVICE = '64A70010-F691-4B93-A6F4-0968F5B648F8'
    ORGANIZATION_CHAR = '64A7000B-F691-4B93-A6F4-0968F5B648F8'
    SOFTWARE_CHAR = '64A70013-F691-4B93-A6F4-0968F5B648F8'
    HARDWARE_CHAR = '64A70001-F691-4B93-A6F4-0968F5B648F8'


class _IO(Enum):
    """Enum containing _IO UUIDs"""
    SERVICE = '64A70012-F691-4B93-A6F4-0968F5B648F8'
    BATTERY_CHAR = '64A70007-F691-4B93-A6F4-0968F5B648F8'
    USER_BUTTON_CHAR = '64A7000D-F691-4B93-A6F4-0968F5B648F8'
    VIBRATOR_CHAR = '64A70008-F691-4B93-A6F4-0968F5B648F8'
    LED_CHAR = '64A70009-F691-4B93-A6F4-0968F5B648F8'
    KEEP_ALIVE_CHAR = '64A7000F-F691-4B93-A6F4-0968F5B648F8'


class _SENSOR(Enum):
    """Enum containing sensor UUIDs"""
    SERVICE = '64A70011-F691-4B93-A6F4-0968F5B648F8'
    TEMP_CHAR = '64A70014-F691-4B93-A6F4-0968F5B648F8'
    QUATERNIONS_CHAR = '64A70002-F691-4B93-A6F4-0968F5B648F8'
    RAW_CHAR = '64A7000A-F691-4B93-A6F4-0968F5B648F8'
    MOTION_CHAR = '64A7000C-F691-4B93-A6F4-0968F5B648F8'
    MAGN_CALIBRATE_CHAR = '64A70021-F691-4B93-A6F4-0968F5B648F8'
    QUATERNIONS_RESET_CHAR = '64A70004-F691-4B93-A6F4-0968F5B648F8'


class PATTERN(Enum):
    """Enum for wand vibration patterns"""
    REGULAR = 1
    SHORT = 2
    BURST = 3
    LONG = 4
    SHORT_LONG = 5
    SHORT_SHORT = 6
    BIG_PAUSE = 7


class Wand():
    def __init__(self, debug=False):
        # Meta stuff
        self._dev = None
        self.debug = debug
        # Notification stuff
        self.connected = False
        self._position_callbacks = {}
        self._position_subscribed = False
        self._button_callbacks = {}
        self._button_subscribed = False
        self._notification_thread = None
        self._position_notification_handle = 41
        self._button_notification_handle = 33

        # services
        self._io_vibra_char = None
        self._sensor_quaternions_char = None

    def scan(self):
        # 扫描设备
        timeout = 5000
        print("Scanning for {} seconds...".format(timeout))
        self._prefix = "Kano-Wand"
        adapters = simplepyble.Adapter.get_adapters()

        if len(adapters) == 0:
            print("No adapters found")

        # Query the user to pick an adapter
        print("Please select an adapter:")
        for i, adapter in enumerate(adapters):
            print(f"{i}: {adapter.identifier()} [{adapter.address()}]")

        choice = int(input("Enter choice: "))
        adapter = adapters[choice]

        print(f"Selected adapter: {adapter.identifier()} [{adapter.address()}]")

        adapter.set_callback_on_scan_start(lambda: print("Scan started."))
        adapter.set_callback_on_scan_stop(lambda: print("Scan complete."))
        # adapter.set_callback_on_scan_found(
        #     lambda peripheral: print(f"Found {peripheral.identifier()} [{peripheral.address()}]"))

        # Scan for 5 seconds
        adapter.scan_for(timeout)
        peripherals = adapter.scan_get_results()

        # Query the user to pick a peripheral
        print("Please select a peripheral:")
        for i, peripheral in enumerate(peripherals):
            identifier = peripheral.identifier()
            address = peripheral.address()
            if identifier:
                print(f"{i}: {identifier} [{address}]")

        choice = int(input("Enter choice: "))
        peripheral = peripherals[choice]
        self._dev = peripheral
        self.name = f"{peripheral.identifier()} [{peripheral.address()}]"

    def connect(self):
        self.scan()
        print("Connecting to {}...".format(self.name))

        if not self._dev:
            print("No connected device")
            return None

        # 连接外围设备
        self._dev.connect()
        self._lock = threading.Lock()
        self.connected = True
        print("Connected to {}".format(self.name))

        # 列出service
        print("Services to {}".format(self.name))
        services = self._dev.services()
        for service in services:
            print(f"Service: {service.uuid()}")
            for characteristic in service.characteristics():
                print(f"    Characteristic: {characteristic.uuid()}")
                uuid = characteristic.uuid()
                if uuid == _IO.VIBRATOR_CHAR.value:
                   self._io_vibra_char = characteristic
                if uuid == _SENSOR.QUATERNIONS_CHAR.value:
                   self._sensor_quaternions_char = characteristic


        # position notify
        # self._dev.notify(_SENSOR.SERVICE.value.lower(), _SENSOR.QUATERNIONS_CHAR.value.lower(),lambda data: print(f"Notification position: {data}"))
        # button notify
        # self._dev.notify(_IO.SERVICE.value.lower(), _IO.USER_BUTTON_CHAR.value.lower(), lambda data: print(f"Notification button: {data}"))


        while(True):
            time.sleep(1)
            contents = self._dev.read(_IO.SERVICE.value.lower(), _IO.USER_BUTTON_CHAR.value.lower())
            print(f"Contents: {contents}")
        # self.post_connect()


    def get_service_by_uuid(self, service_uuid=''):
        # 根据uuid获取Service
        pass

    def writeCharacteristic(self):
        pass

    def post_connect(self):
        """Do anything necessary after connecting
        """
        self.vibrate(PATTERN.BURST)

    def disconnect(self):
        self._dev.disconnect()
        self.connected = False
        self._position_subscribed = False
        self._button_subscribed = False
        self._temperature_subscribed = False
        self._battery_subscribed = False
        self.post_disconnect()
        print("Disconnected from {}".format(self.name))

    def post_disconnect(self):
        """Do anything necessary after disconnecting
        """
        pass

    def vibrate(self, pattern=PATTERN.REGULAR):
        """Vibrate wand with pattern

        Keyword Arguments:
            pattern {kano_wand.PATTERN} -- Vibration pattern (default: {PATTERN.REGULAR})

        Returns {bytes} -- Status
        """
        with self._lock:
            if isinstance(pattern, PATTERN):
                message = [pattern.value]
            else:
                message = [pattern]

            if self.debug:
                print("Setting vibrate to {}".format(message))

            if not hasattr(self, "_vibrator_handle"):
                handle = self._io_service.getCharacteristics(_IO.VIBRATOR_CHAR.value)[0]
                self._vibrator_handle = handle.getHandle()
            return self.writeCharacteristic(self._vibrator_handle, bytes(message), withResponse=True)

    # SENSORS
    def on(self, event, callback):
        """Add an event listener

        Arguments:
            event {str} -- Event type, "position", "button", "temp", or "battery"
            callback {function} -- Callback function

        Returns {str} -- ID of the callback for removal later
        """
        if self.debug:
            print("Adding callback for {} notification...".format(event))

        id = None
        if event == "position":
            id = uuid.uuid4()
            self._position_callbacks[id] = callback
            self.subscribe_position()
        elif event == "button":
            id = uuid.uuid4()
            self._button_callbacks[id] = callback
            self.subscribe_button()

        return id

    def off(self, uuid, continue_notifications=False):
        """Remove a callback

        Arguments:
            uuid {str} -- Remove a callback with its id

        Keyword Arguments:
            continue_notifications {bool} -- Keep notification thread running (default: {False})

        Returns {bool} -- If removal was successful or not
        """
        removed = False
        if self._position_callbacks.get(uuid) != None:
            removed = True
            self._position_callbacks.pop(uuid)
            if len(self._position_callbacks.values()) == 0:
                self.unsubscribe_position(continue_notifications=continue_notifications)
        elif self._button_callbacks.get(uuid) != None:
            removed = True
            self._button_callbacks.pop(uuid)
            if len(self._button_callbacks.values()) == 0:
                self.unsubscribe_button(continue_notifications=continue_notifications)

        if self.debug:
            if removed:
                print("Removed callback {}".format(uuid))
            else:
                print("Could not remove callback {}".format(uuid))

        return removed

    def subscribe_position(self):
        """Subscribe to position notifications and start thread if necessary
        """
        if self.debug:
            print("Subscribing to position notification")

        self._position_subscribed = True
        with self._lock:
            if not hasattr(self, "_position_handle"):
                handle = self._sensor_service.getCharacteristics(_SENSOR.QUATERNIONS_CHAR.value)[0]
                self._position_handle = handle.getHandle()

            self.writeCharacteristic(self._position_handle + 1, bytes([1, 0]))
        self._start_notification_thread()

    def unsubscribe_position(self, continue_notifications=False):
        """Unsubscribe to position notifications

        Keyword Arguments:
            continue_notifications {bool} -- Keep notification thread running (default: {False})
        """
        if self.debug:
            print("Unsubscribing from position notification")

        self._position_subscribed = continue_notifications
        with self._lock:
            if not hasattr(self, "_position_handle"):
                handle = self._sensor_service.getCharacteristics(_SENSOR.QUATERNIONS_CHAR.value)[0]
                self._position_handle = handle.getHandle()

            self.writeCharacteristic(self._position_handle + 1, bytes([0, 0]))

    def subscribe_button(self):
        """Subscribe to button notifications and start thread if necessary
        """
        if self.debug:
            print("Subscribing to button notification")

        self._button_subscribed = True
        with self._lock:
            if not hasattr(self, "_button_handle"):
                handle = self._io_service.getCharacteristics(_IO.USER_BUTTON_CHAR.value)[0]
                self._button_handle = handle.getHandle()

            self.writeCharacteristic(self._button_handle + 1, bytes([1, 0]))
        self._start_notification_thread()

    def unsubscribe_button(self, continue_notifications=False):
        """Unsubscribe to button notifications

        Keyword Arguments:
            continue_notifications {bool} -- Keep notification thread running (default: {False})
        """
        if self.debug:
            print("Unsubscribing from button notification")

        self._button_subscribed = continue_notifications
        with self._lock:
            if not hasattr(self, "_button_handle"):
                handle = self._io_service.getCharacteristics(_IO.USER_BUTTON_CHAR.value)[0]
                self._button_handle = handle.getHandle()

            self.writeCharacteristic(self._button_handle + 1, bytes([0, 0]))

    def _start_notification_thread(self):
        try:
            if self._notification_thread == None:
                self.reset_position()
                self._notification_thread = threading.Thread(target=self._notification_wait)
                self._notification_thread.start()
        except:
            pass

    def _notification_wait(self):
        if self.debug:
            print("Notification thread started")

        while (self.connected and
               (self._position_subscribed or
                self._button_subscribed or
                self._temperature_subscribed or
                self._battery_subscribed)):
            try:
                if super().waitForNotifications(1):
                    continue
            except:
                continue

        if self.debug:
            print("Notification thread stopped")

    def _on_position(self, data):
        """Private function for position notification

        Arguments:
            data {bytes} -- Data from device
        """
        # I got part of this from Kano's node module and modified it
        y = numpy.int16(numpy.uint16(int.from_bytes(data[0:2], byteorder='little')))
        x = -1 * numpy.int16(numpy.uint16(int.from_bytes(data[2:4], byteorder='little')))
        w = -1 * numpy.int16(numpy.uint16(int.from_bytes(data[4:6], byteorder='little')))
        z = numpy.int16(numpy.uint16(int.from_bytes(data[6:8], byteorder='little')))

        if self.debug:
            pitch = "Pitch: {}".format(z).ljust(16)
            roll = "Roll: {}".format(w).ljust(16)
            print("{}{}(x, y): ({}, {})".format(pitch, roll, x, y))

        self.on_position(x, y, z, w)
        for callback in self._position_callbacks.values():
            callback(x, y, z, w)

    def on_position(self, roll, x, y, z):
        """Function called on position notification

        Arguments:
            x {int} -- X position of wand (Between -1000 and 1000)
            y {int} -- Y position of wand (Between -1000 and 1000)
            pitch {int} -- Pitch of wand (Between -1000 and 1000)
            roll {int} -- Roll of wand (Between -1000 and 1000)
        """
        pass

    def reset_position(self):
        """Reset the quaternains of the wand
        """
        handle = self._sensor_service.getCharacteristics(_SENSOR.QUATERNIONS_RESET_CHAR.value)[0].getHandle()
        with self._lock:
            self.writeCharacteristic(handle, bytes([1]))

    def _on_button(self, data):
        """Private function for button notification

        Arguments:
            data {bytes} -- Data from device
        """
        val = data[0] == 1

        if self.debug:
            print("Button: {}".format(val))

        self.on_button(val)
        for callback in self._button_callbacks.values():
            callback(val)

    def on_button(self, value):
        """Function called on button notification

        Arguments:
            pressed {bool} -- If button is pressed
        """
        pass

    def handleNotification(self, cHandle, data):
        """Handle notifications subscribed to

        Arguments:
            cHandle {int} -- Handle of notification
            data {bytes} -- Data from device
        """
        if cHandle == self._position_notification_handle:
            self._on_position(data)
        elif cHandle == self._button_notification_handle:
            self._on_button(data)
